/**
  ******************************************************************************
  * @file           : bsp_mpu6050.c
  * @author         : Ylint
  * @brief          : None
  * @attention      : None
  * @date           : 2024/4/6
  ******************************************************************************
  */

#include "bsp_mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "stdio.h"
#include "math.h"
#include "bsp_control.h"


//Pitch（俯仰角）、Roll（横滚角）、Yaw（偏航角）。
float Pitch, Roll, Yaw;
short Gyro[3];
short Accel[3];


static signed char gyro_orientation[9] = { -1, 0, 0, 0, -1, 0, 0, 0, 1 };

/**
  * @brief  官方自检例程，为MPU6050_DMP_Init函数服务。
  * @param  None
  * @retval 0 if successful.
  */
static int run_self_test(void)
{
    int result;
    long gyro[3], accel[3];

    result = mpu_run_self_test(gyro, accel);
    if (result == 0x7)
//    if (result == 0x3)
    {
        unsigned short accel_sens;
        float gyro_sens;

        mpu_get_accel_sens(&accel_sens);
        accel[0] *= accel_sens;
        accel[1] *= accel_sens;
        accel[2] *= accel_sens;
        dmp_set_accel_bias(accel);
        mpu_get_gyro_sens(&gyro_sens);
        gyro[0] = (long) (gyro[0] * gyro_sens);
        gyro[1] = (long) (gyro[1] * gyro_sens);
        gyro[2] = (long) (gyro[2] * gyro_sens);
        dmp_set_gyro_bias(gyro);
    } else{
        return -1;  //存在问题
    }
    return 0;   //正常
}

/**
  * @brief  官方例程，为MPU6050_DMP_Init函数服务。
  * @param  row：
  * @retval b：
  */
static unsigned short inv_row_2_scale(const signed char *row) {
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;      // error
    return b;
}

/**
  * @brief  官方例程，为MPU6050_DMP_Init函数服务。
  * @param  mtx：
  * @retval scalar：
  */
static unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) {
    unsigned short scalar;
    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;

    return scalar;
}

/**
  * @brief  MPU6050 DMP初始化。
  * @param  None
  * @retval 0 if successful.
  */
int MPU6050_DMP_Init(void)
{
    if (!mpu_init(NULL)) {
        if (mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) != 0)     //设置传感器
        {
            return 1;
        }
        if (mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) != 0)      //设置FIFO
        {
            return 2;
        }
        if (mpu_set_sample_rate(DEFAULT_MPU_HZ) != 0)           //设置采样频率
        {
            return 3;
        }
        if (dmp_load_motion_driver_firmware() != 0)     //加载DMP固件
        {
            return 4;
        }
        if (dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)) != 0)        //设置陀螺仪方向
        {
            return 5;
        }
        if (dmp_enable_feature(
                DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
                DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL
                | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL) != 0)        //设置DMP功能
        {
            return 6;
        }
        if (dmp_set_fifo_rate(DEFAULT_MPU_HZ) != 0)     //设置输出速率
        {
            return 7;
        }
        if(run_self_test() != 0)        //自检
        {
            return 8;
        }
        if (mpu_set_dmp_state(1) != 0)
        {
            return 9;
        }
        return 0;       //MPU6050 DMP初始化成功
    }
}

/**
  * @brief  获取姿态角度。Pitch（俯仰角）、Roll（横滚角）、Yaw（偏航角）。
  * @param  pitch：存储 Pitch（俯仰角）的地址。
  * @param  roll：存储 Roll（横滚角）的地址。
  * @param  yaw：存储 Yaw（偏航角）的地址。
  * @retval 0 if successful.
  */
int MPU6050_DMP_Get_Data(float *pitch, float *roll, float *yaw)
{
    float Q[4]={0.0f};
//    short gyro[3];
//    short accel[3];
    long quat[4];
    unsigned long timestamp;
    short sensors;
    unsigned char more;
    if(dmp_read_fifo(Gyro, Accel, quat, &timestamp, &sensors, &more) != 0)
    {
        return -1;  //数据获取失败
    }
    if(sensors & INV_WXYZ_QUAT)
    {
        Q[0] = quat[0] / Q30;
        Q[1] = quat[1] / Q30;
        Q[2] = quat[2] / Q30;
        Q[3] = quat[3] / Q30;


        *pitch  = asin(-2.0f * (Q[1] * Q[3] - Q[0] * Q[2])) * 57.3;
        *roll   = atan2(2.0f * (Q[0] * Q[1] + Q[2] * Q[3]), Q[0] * Q[0] - Q[1] * Q[1] - Q[2] * Q[2] + Q[3] * Q[3]) * 57.3;
        *yaw    = atan2(2 *(Q[1] * Q[2] + Q[0] * Q[3]), Q[0] * Q[0] + Q[1] * Q[1] - Q[2] * Q[2] - Q[3] * Q[3]) * 57.3;
    }

    return 0;   //成功获取数据
}

/**
  * @brief  MPU605_INT引脚中断回调函数。
  * @param  GPIO_Pin：引脚位。
  * @retval None
  */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if(GPIO_Pin == MPU6050_INT_Pin)
  {
    Motor_Control();
  }
}



